/**
  ****************************(C)2023 Kalman Filter****************************
  * @file       kalmanFilter.c
  * @brief      卡尔曼滤波函数实现
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023-3-31     Shen Jiewen     1. 测试阶段
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C)2023 Kalman Filter****************************
  */
#include "kalmanFilter.h"
#include "headfile.h"

// 初始化卡尔曼滤波器状态
void kalman_init(kalman_state *state, double q, double r, double p, double initial_value)
{
    state->q = q; // 设置过程噪声协方差
    state->r = r; // 设置测量噪声协方差
    state->p = p; // 设置初始估计误差协方差
    state->x = initial_value; // 设置初始值
}

// 更新卡尔曼滤波器状态
void kalman_update(kalman_state *state, double measurement)
{
    // 预测下一时刻的值
    state->p = state->p + state->q;

    // 计算卡尔曼增益
    state->k = state->p / (state->p + state->r);

    // 更新实际值
    state->x = state->x + state->k * (measurement - state->x);

    // 更新估计误差协方差
    state->p = (1 - state->k) * state->p;
}
